package pacman.entries.pacman;

import java.util.Arrays;

public class LP
{
	//you can also probably just trust this to work -_-
	private final static double EPSILON = 1E-5;

	/**
	 * Calculate the optimal solution of a linear program in standard form: minimize objective <b>c</b><b>x</b>, subject to constraints <b>A</b><b>x</b> = <b>b</b> and <b>x</b> >= 0. This implementation uses the simplex algorithm with Bland's rule.
	 * 
	 * @param objective
	 *            double[] of the coefficients of the objective function, discluding slack variables
	 * @param constraints
	 *            double[nConstraints][] of the coefficients of the linearly independent constraints, including slack variables (A augmented with b)
	 * @return Solution including slack variables
	 */
	public static double[] calculate(double[] objective, double[][] constraints)
	{
		double[][] tableau = new double[constraints.length + 1][constraints[0].length + 1]; //nRows by nColumns
		int height = tableau.length, width = tableau[0].length;
		tableau[0][0] = 1d;
		for (int i = 0; i < objective.length; i++)
			tableau[0][i + 1] = -objective[i];
		for (int r = 1; r < height; r++)
			System.arraycopy(constraints[r - 1], 0, tableau[r], 1, width - 1);

		//calculate basis and price out
		int[] basis = new int[height - 1];
		outer: for (int c = 1; c < width - 1; c++)
		{
			int foundRow = -1;
			for (int r = 1; r < height; r++)
			{
				if (Math.abs(tableau[r][c]) > EPSILON)
				{
					if (Math.abs(tableau[r][c] - 1) > EPSILON || foundRow >= 0)
						continue outer;
					else
						foundRow = r;
				}
			}
			if (foundRow == -1)
				continue;
			basis[foundRow - 1] = c;
			//price out
			double factor = -tableau[0][c];
			if (Math.abs(factor) > EPSILON)
				for (int i = 1; i < width; i++)
					tableau[0][i] += tableau[foundRow][i] * factor;
		}
		int nBasic = 0;
		for (int r = 0; r < height - 1; r++)
			if (basis[r] != 0)
				nBasic++;

		//phase 1
		if (nBasic < height - 1)
		{
			//add artificial variables: see http://www2.isye.gatech.edu/~spyros/LP/node23.html
			int artificialWidth = width + (height - 1 - nBasic); //add the number of artificial variables needed (how many basic variables are lacking)

			double[] origTopRow = new double[width];
			System.arraycopy(tableau[0], 0, origTopRow, 0, width);
			tableau[0] = Arrays.copyOf(tableau[0], artificialWidth);
			for (int i = 1; i < width - 1; i++)
				tableau[0][i] = 0;
			for (int i = width - 1; i < artificialWidth - 1; i++)
				tableau[0][i] = -1;

			int artificialCol = width - 1;
			for (int r = 1; r < height; r++)
			{
				tableau[r] = Arrays.copyOf(tableau[r], artificialWidth);
				tableau[r][artificialWidth - 1] = tableau[r][width - 1];
				tableau[r][width - 1] = 0;
				if (basis[r - 1] == 0)
				{
					tableau[r][artificialCol] = 1;
					basis[r - 1] = artificialCol;
					//price out
					for (int c = 1; c < artificialWidth; c++)
						tableau[0][c] += tableau[r][c];
					artificialCol++;
				}
			}

			calculate(tableau, basis);

			//drop artificial variables
			for (int r = 1; r < height; r++)
			{
				tableau[r][width - 1] = tableau[r][artificialWidth - 1];
				tableau[r] = Arrays.copyOf(tableau[r], width);
			}
			tableau[0] = origTopRow;
			//price out
			for (int r = 1; r < height; r++)
			{
				double factor = -tableau[0][basis[r - 1]];
				if (Math.abs(factor) > EPSILON)
					for (int c = 1; c < width; c++)
						tableau[0][c] += tableau[r][c] * factor;
			}
		}

		//phase 2
		calculate(tableau, basis);

		double[] solution = new double[width - 2];
		for (int r = 1; r < height; r++)
		{
			//basis[row] returns the column of the basic (nonzero) variable of that row
			//the last column contains its value
			solution[basis[r - 1] - 1] = tableau[r][width - 1];
		}
		return solution;
	}

	private static void calculate(double[][] tableau, int[] basis)
	{
		int height = tableau.length, width = tableau[0].length;
		while (true)
		{
			//find pivot column, Bland's rule
			int pivotCol = 0;
			for (int c = 1; c < width - 1; c++)
			{
				if (tableau[0][c] > EPSILON)
				{
					pivotCol = c;
					break;
				}
			}

			//optimality condition: all the "costs" (coefficients at the top besides the first and last column) are nonpositive
			if (pivotCol == 0)
				return;

			//find pivot row
			int pivotRow = 0;
			double temp = Double.POSITIVE_INFINITY;
			for (int r = 1; r < height; r++)
			{
				if (tableau[r][pivotCol] > EPSILON)
				{
					double ratio = tableau[r][width - 1] / tableau[r][pivotCol];
					//in the event of a tie, choose the row whose corresponding basic variable is leftmost
					if (ratio < temp - EPSILON || (ratio < temp + EPSILON && basis[r - 1] < basis[pivotRow - 1]))
					{
						pivotRow = r;
						temp = ratio;
					}
				}
			}
			//in theory, a pivotRow should always be able to be found

			//pivot
			basis[pivotRow - 1] = pivotCol;
			pivot(tableau, pivotRow, pivotCol);
		}
	}

	private static void pivot(double[][] tableau, int pivotRow, int pivotCol)
	{
		double pivot = tableau[pivotRow][pivotCol];
		int height = tableau.length, width = tableau[0].length;
		//divide this row by the pivot to make the pivot 1
		for (int c = 1; c < width; c++)
			tableau[pivotRow][c] /= pivot;
		//make the other cells in the pivot's column 0 by adding multiples of this row
		for (int r = 0; r < height; r++)
		{
			if (r != pivotRow)
			{
				double factor = -tableau[r][pivotCol]; //(we know the pivot must be 1)
				for (int c = 1; c < width; c++)
					tableau[r][c] += tableau[pivotRow][c] * factor;
			}
		}
	}
}
